#!/usr/bin/env python3

import rospy
from mavros_msgs.srv import CommandBool, SetMode, CommandVtolTransition, CommandLong, CommandVtolTransitionRequest, CommandHome
from mavros_msgs.msg import State, PositionTarget, ExtendedState
from geometry_msgs.msg import PoseStamped, TwistStamped,Vector3
from geographic_msgs.msg import GeoPointStamped
import tf.transformations as tf
from std_msgs.msg import Header, String, Int32
from sensor_msgs.msg import NavSatFix
import sys, math, time, openpyxl
import numpy as np


class L1:
    def __init__(self):
        
        """
        init
        """
        self.vehicle_type = 'uav'
        self.id = uav_id    #注意id号为1，输入
        # print(self.id)
        
        self.g = 9.81
        self.q_2, self.dq_2 = 0.0, 0.0
        self.r_2, self.dr_2 = 0.0, 0.0
        self.q_lon3, self.q_lat3 = 0.0, 0.0
        self.dq_lon3, self.dq_lat3 = 0.0, 0.0

        self.target_motion = PositionTarget()
        #制导指令初始化
        self.v = 0.0
        self.roll = 0.0
        self.alt = 0.0
        
        self.flag = 0
        self.scene = 0
        self.um = 0.0     #二维输出
        
        #集结
        self.vs = 142.0     # 修改集结速度，一圈时间54.63
        self.print_h = 1.0
        self.d_m = 270.0
        self.t_last_loop = 0.0      #初始时间
        self.t_now_loop = 0.0    #目前时间
        
        self.state = State()
        self.vtol_state = ExtendedState()
        self.gps_received = False
        # self.local_pose = PoseStamped()
        # self.global_pose = NavSatFix()

        self.num = 5 # 无人机数量
        self.local_poses = [PoseStamped() for i in range(self.num)]
        self.local_vels = [TwistStamped() for i in range(self.num)]
        self.global_poses = [NavSatFix() for i in range(self.num)]
        self.local_poses_gps = [PoseStamped() for i in range(self.num)]
        #communication_ids = np.argwhere(self.A[self.id, :] == 1)#找出为1的元素索引id，为二维数组
        #self.communication_ids = communication_ids.flatten().tolist()#转为一维数组并转换为列表，与本机有通信的id(如果id从0开始)
        self.begin_lat = 0.0
        self.begin_lon = 0.0
        self.begin_alt = 0.0

        '''
        ros subscribers
        '''
        #订阅自身信息
        rospy.Subscriber('/command', Int32, self.command_callback)
        self.state_sub = rospy.Subscriber(self.vehicle_type + str(self.id) + '/mavros/state', State, self.state_callback)
        self.vtol_state_sub = rospy.Subscriber(self.vehicle_type  + str(self.id) + '/mavros/extended_state', ExtendedState, self.vtol_state_callback)#垂起状态
        self.local_pose_own_sub = rospy.Subscriber(self.vehicle_type+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback_own,queue_size=1)
        self.local_vel_own_sub = rospy.Subscriber(self.vehicle_type+str(self.id)+"/mavros/local_position/velocity_local", TwistStamped, self.local_vel_callback_own,queue_size=1)
        self.global_pose_own_sub = rospy.Subscriber(self.vehicle_type + str(self.id) + '/mavros/global_position/global', NavSatFix, self.global_pose_callback_own,queue_size=1)
        #订阅其他无人机信息：位置
        #self.local_pose_other_sub = [None]*len(self.communication_ids)
        #for nm in range(len(self.communication_ids)):
            #self.local_pose_other_sub[nm] = rospy.Subscriber(self.vehicle_type+'_'+str(self.communication_ids[nm])+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback_other,queue_size=1,callback_args=nm)
            

        ''' 
        ros publishers
        '''
        self.waypoint_pub = rospy.Publisher(self.vehicle_type+str(self.id)+'/mavros/setpoint_position/local', PoseStamped, queue_size=1 )
        self.target_motion_pub = rospy.Publisher(self.vehicle_type+ str(self.id) + "/mavros/setpoint_raw/local", PositionTarget, queue_size=1)
        self.gps_origin_pub = rospy.Publisher(self.vehicle_type + str(self.id) + "/mavros/global_position/set_gp_origin", GeoPointStamped, queue_size=1)
        self.command_L1_pub = rospy.Publisher('/command_L1',Int32, queue_size=1)
        '''
        ros services
        '''
        self.arm_service = rospy.ServiceProxy(self.vehicle_type+str(self.id)+'/mavros/cmd/arming', CommandBool)
        self.set_mode_service = rospy.ServiceProxy(self.vehicle_type+str(self.id)+'/mavros/set_mode', SetMode)      #self.flightModeService
        self.transition_service = rospy.ServiceProxy(self.vehicle_type + str(self.id) + '/mavros/cmd/vtol_transition',CommandVtolTransition)
        self.command_service = rospy.ServiceProxy(self.vehicle_type+str(self.id)+'/mavros/cmd/command', CommandLong)

    
        """
        constants and variables
        """
        self.phase_enum = {
            "MC_TAKEOFF": 0,
            "MC_TO_FW": 1,
            "FW_OFFBOARD": 2,
            "FW_MISSION_WP1": 3,
            "FW_FORMATION": 4,
            "FW_TO_MC": 5,
            "MC_LAND": 6,
            "RETURN": 7,
            "FINISH": 8,
            "FW_L1": 9,
            "FW_guidance": 10
        }
        self.current_phase = 11
        self.start_time = rospy.Time.now()

        # TODO: 和wait_for_connection有重复，待优化，可以把赋值放到后面和connection类似，设置原点可以考虑在初始化完成因为要发多次
        while not self.gps_received and not rospy.is_shutdown():
            print("uav_" + str(self.id) + ": Waiting for valid GPS data...")
            rospy.sleep(1)
        self.begin_x = 0.0
        self.begin_y = 0.0
        self.begin_z = 10.0
        self.begin_lat = self.global_poses[self.id].latitude # 4277582.939803197
        self.begin_lon = self.global_poses[self.id].longitude
        self.begin_alt = self.global_poses[self.id].altitude
        self.set_gps_origin(self.begin_lat, self.begin_lon, self.begin_alt)#47.397742, 8.5455938, 535.3119158897637)
        self.WP1 = np.array([250.0, 100.0, 30.0])
        self.V1 = np.array([10.0, 4.0, 0.0])               
        
        print("communication of tracking is initialized")



    """
    Callback functions for subscribers.
    """  
    def state_callback(self, msg):
        self.state = msg

    def vtol_state_callback(self, msg):
        self.vtol_state = msg

    def local_pose_callback_own(self, msg):
        self.local_poses[self.id] = msg

    def local_vel_callback_own(self, msg):
        self.local_vels[self.id] = msg
    
    def global_pose_callback_own(self, msg):
        self.global_poses[self.id] = msg
        if msg.status.status >= 0:  # 确保GPS数据有效
            self.gps_received = True
        # print(self.global_pose.latitude, self.global_pose.longitude, self.global_pose.altitude)
        
        lat = msg.latitude
        lon = msg.longitude
        ref_lat = self.begin_lat
        ref_lon = self.begin_lon
        x,y = self.gps2xy(lat, lon, ref_lat, ref_lon)
        self.local_poses_gps[self.id].pose.position.x = x # Eath(m)
        self.local_poses_gps[self.id].pose.position.y = y # North(m)
        self.local_poses_gps[self.id].pose.position.z = msg.altitude - self.begin_alt  # Altitude(m)

    def local_pose_callback_other(self, msg, nm):
        self.local_poses[self.communication_ids[nm]] = msg
    

    def wait_for_connection(self):
        while not rospy.is_shutdown() and not self.state.connected:
            print(f"uav_{self.id}: Waiting for FCU connection...")
            rate.sleep()

    
    # def set_offboard_mode(self):
    #     try:
    #         self.set_mode_service(0, 'OFFBOARD')
    #     except rospy.ServiceException as e:
    #         rospy.logerr(f"uav_{self.id}: Set mode failed: {e}")

    def arm(self):
        if self.arm_service(True):
            # print('arm ok!')
            return True
        else:
            print(self.vehicle_type  + str(self.id) + ": arming failed!")
            return False

    def disarm(self):
        if self.arm_service(False):
            return True
        else:
            print(self.vehicle_type + str(self.id) + ": disarming failed!")
            return False

    def set_vtol_state(self, state):
        header = Header()  # 创建一个 Header 实例，可根据需要设置其具体属性
        request = CommandVtolTransitionRequest(header=header, state=state)
        if self.transition_service(request):
            return True
        else:
            print(self.vehicle_type + str(self.id) + ": VTOL state change failed!")
            return False

    def flight_mode_switch(self, flight_mode):
        if self.set_mode_service(base_mode=0, custom_mode=flight_mode):
            print(self.vehicle_type + str(self.id) + " mode switch to: " + flight_mode)
            return True
        else:
            print(self.vehicle_type + str(self.id) + flight_mode + ": Set mode failed!")
            return False
    
    def set_gps_origin(self, latitude, longitude, altitude):
        origin = GeoPointStamped()
        origin.header.stamp = rospy.Time.now()
        origin.position.latitude = latitude
        origin.position.longitude = longitude
        origin.position.altitude = altitude
        for _ in range(5):  # 发布20次，设置成功  TODO：可加入判断是否设置成功，订阅话题gp_origin/但需要将ECEF坐标转换回LLA坐标
            self.gps_origin_pub.publish(origin)
        print(f"uav_{self.id}: GPS origin set to: {latitude}, {longitude}, {altitude}")
        
        # if self.set_orign_service(latitude, longitude, altitude):
        #     return True
        # else:
        #     print(self.vehicle_type + '_' + str(self.id) + ": GPS origin set failed!")
        #     return False
        
        # try:
        #     set_origin_service = rospy.ServiceProxy('/mavros/global_position/set_gp_origin', CommandHome)
        #     response = set_origin_service(latitude, longitude, altitude)
        #     if response.success:
        #         rospy.loginfo("GPS origin set successfully")
        #     else:
        #         rospy.logerr("Failed to set GPS origin")
        # except rospy.ServiceException as e:
        #     rospy.logerr("Service call failed: %s" % e)


    # def takeoff(self):
    #     pose = PoseStamped()
    #     pose.pose.position.x = self.begin_x
    #     pose.pose.position.y = self.begin_y
    #     pose.pose.position.z = self.begin_z
    #     self.waypoint_pub.publish(pose)

    def takeoff_and_arm(self):
        #self.set_gps_origin(self.begin_lat, self.begin_lon, self.begin_alt)#47.397742, 8.5455938, 535.3119158897637)
        self.set_vtol_state(state=3)  # Change to multi-copter mode
        self.flight_mode_switch('AUTO.TAKEOFF')
        self.arm()
        

    def control(self):
        if self.current_phase == self.phase_enum["MC_TAKEOFF"]:
            self.takeoff_and_arm()
            print(f"uav_{self.id}: MC_TAKEOFF")
            self.current_phase = self.phase_enum["MC_TO_FW"]
            # print(self.current_phase)
            
        elif self.current_phase == self.phase_enum["MC_TO_FW"]:
            #if self.reached_altitude(self.begin_z):
            if self.state.mode == 'AUTO.LOITER' and self.reached_altitude(10): 
                self.set_vtol_state(state=4)  # Change to fixed-wing mode
                print(f"uav_{self.id}: MC_TO_FW")
                self.current_phase = self.phase_enum["FW_OFFBOARD"]
            # else:
            #     print('state.mode: ', self.state.mode)
        
        elif self.current_phase == self.phase_enum["FW_OFFBOARD"]:
            if self.vtol_state.vtol_state == ExtendedState.VTOL_STATE_FW:
                self.publish_target_setpoint(position_sp=self.WP1, velocity_sp=self.V1, motion_type=1)
                self.flight_mode_switch('OFFBOARD')
                print(f"uav_{self.id}: FW_OFFBOARD")
                # print(f"uav_{self.id}: FW_MISSION_WP1")
                self.current_phase = self.phase_enum["FW_L1"]

        # elif self.current_phase == self.phase_enum["FW_MISSION_WP1"]:   #不需要
        #         self.publish_target_setpoint(position_sp=self.WP1, velocity_sp=self.V1, motion_type=1) #用该话题发布航点到达后不会盘旋，一直往前飞
        #         # self.fly_to_waypoint(self.WP1[0], self.WP1[1], self.WP1[2]) #用该话题发布航点到达后会盘旋
        #         if self.reached_waypoint(self.WP1[0], self.WP1[1], self.WP1[2]):
        #             self.current_phase = self.phase_enum["FW_FORMATION"]
        
        # TODO: formation control 待优化
        elif self.current_phase == self.phase_enum["FW_L1"]:
            #L1制导律
            # print(f"uav_{self.id}: FW_L1")
            global cnt,pn
            # print(self.current_phase)
            self.L1_control(cnt,self.id,1)#TODO：防碰撞未加
            if math.sqrt(((xt[cnt+1] - self.local_poses_gps[self.id].pose.position.x)**2+ (yt[cnt+1] - self.local_poses_gps[self.id].pose.position.y)**2))<50:
                cnt=cnt + 1
                if cnt>=pn:
                    self.flag = 10
                    
            if self.flag != 10:
               self.publish_target_setpoint(velocity_sp=np.array([self.v, self.roll, self.alt]))
            if self.flag == 10:
                self.command_L1_pub.publish(2)
                print('L1 over!')
                cnt = 0
            #    self.publish_target_setpoint(velocity_sp=np.array([self.v, self.roll, 30]))
            #    print(np.array([self.v, self.roll, self.alt]))
            #TODO：编队控制结束判断，进入下一阶段
            if self.reached_waypoint(1400, 50, 30):
                self.current_phase = self.phase_enum["RETURN"]#编队后RETURN模式不会回到起点？和RTL_TYPE参数有关，0123四种模式

        elif self.current_phase == self.phase_enum["RETURN"]:
            self.flight_mode_switch('AUTO.RTL')
            print(f"uav_{self.id}: FW_RETURN")
            self.current_phase = self.phase_enum["FINISH"]
        
        # elif self.current_phase == self.phase_enum["FW_TO_MC"]:
        #     if self.reached_waypoint(self.WP1[0], self.WP1[1], self.WP1[2]):
        #         self.flight_mode_switch('POSCTL')
        #         self.set_vtol_state(state=3)  # Change to multi-copter mode
        #         print(f"uav_{self.id}: FW_TO_MC")
        #         self.current_phase = self.phase_enum["MC_LAND"]
            
        # elif self.current_phase == self.phase_enum["MC_LAND"]:
        #     if self.vtol_state.vtol_state == ExtendedState.VTOL_STATE_MC:  # 进入MC_LAND阶段
        #         self.flight_mode_switch('AUTO.LAND')
        #         print(f"uav_{self.id}: MC_LAND")
        #         self.current_phase = self.phase_enum["FINISH"]
        #     # if self.reached_altitude(0):
        #     #     self.current_state = self.state_enum["INIT"]

        elif self.current_phase == self.phase_enum["FINISH"]:
            print(f"uav_{self.id}: FINISH")
            rospy.signal_shutdown("Mission complete")  # 关闭ROS节点
            sys.exit(0)  # 退出程序

    def reached_altitude(self, target_altitude):
        return abs(self.local_poses_gps[self.id].pose.position.z - target_altitude) < 1.0

    def reached_waypoint(self, x, y, z):
        return (abs(self.local_poses_gps[self.id].pose.position.x - x) < 10.0 and
                abs(self.local_poses_gps[self.id].pose.position.y - y) < 10.0 and
                abs(self.local_poses_gps[self.id].pose.position.z - z) < 5.0)

    """
    Functions for publishing topics.
    """
    def fly_to_waypoint(self, x, y, z):
        pose = PoseStamped()
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z
        self.waypoint_pub.publish(pose)
    
    def publish_target_setpoint(self, **kwargs):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED #1
        motion_type = kwargs.get("motion_type", 0)
        if motion_type == 0: # v-roll-alt control
            msg.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
                            + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                            + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE 
        elif motion_type == 1: # position + velocity control
            msg.type_mask = PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
                            + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE 
        position_sp = kwargs.get("position_sp", np.nan * np.zeros(3))
        velocity_sp = kwargs.get("velocity_sp", np.nan * np.zeros(3))
        msg.position.x = position_sp[0]
        msg.position.y = position_sp[1]
        msg.position.z = position_sp[2]
        msg.velocity.x = velocity_sp[0] 
        msg.velocity.y = velocity_sp[1]
        msg.velocity.z = velocity_sp[2]
        msg.yaw = kwargs.get("yaw_sp", float('nan'))
        msg.yaw_rate = kwargs.get("yaw_rate_sp", float('nan'))
        self.target_motion_pub.publish(msg)
        
    """
    Functions for control.
    """
        
    def L1_control(self,t,id,print_flag):
        
        global xt,yt,seq_begin,flag
        local_pose = PoseStamped()
        local_vels = TwistStamped()
        '''
        if id == '0':
            local_pose = self.local_pose_0
            local_vel = self.local_vel_0
        elif id == '1':
            local_pose = self.local_pose_1
            local_vel = self.local_vel_1
            hx = hx_1;hy = hy_1
        else:
            local_pose = self.local_pose_2
            local_vel = self.local_vel_2
            hx = hx_2;hy = hy_2
        '''
        local_pose = self.local_poses_gps[id]
        local_vels = self.local_vels[id]
        h = 0.02  #控制周期
        vt = 15; #领导者飞行速度 m/s 18
        v_max = 20; #20 m/s 最大飞行速度
        v_min = 10; #10 m/s 最小飞行速度
        roll_m = 0.6; #最大滚转角度 rad
        w_m = 0.5; # 最大航向角速度 rad/s 0.1
        Kv1 = 2.0; #领导者速度控制 0.8 Kv1*速度误差；加速度P控制增益
        # print(id)
        local_angles = self.to_euler_angles(local_pose.pose.orientation.w,local_pose.pose.orientation.x,local_pose.pose.orientation.y,local_pose.pose.orientation.z)
        uav_v = math.pow(local_vels.twist.linear.x ** 2+local_vels.twist.linear.y ** 2+local_vels.twist.linear.z ** 2,1/2)            #无人机空速+local_vel.twist.linear.z ** 2
        # print(id)   
            # print(id)

        self.t_now_loop = rospy.get_time()      #取当前时间
        # print(self.t_now_loop)
        # L1制导程序跟航线
        L1 = 5 * self.vs
        ce0 = [local_pose.pose.position.x, local_pose.pose.position.y]
        r0 = L1
        p1 = [xt[t],yt[t]]
        dn = [xt[t+1] - local_pose.pose.position.x, yt[t+1] - local_pose.pose.position.y]
        #if self.t_now_loop - self.t_last_loop >self.print_h:
            # print('目标航点{}: {}'.format(t+2, [xt[t+1],yt[t+1]]))
            #self.t_last_loop = self.t_now_loop
        fp = [xt[t+1],yt[t+1]]
        inp = self.line_intersect_circle(ce0, r0, p1, fp)      # L1点
        if inp == []:     #无交点
            p_1 = [local_pose.pose.position.x, local_pose.pose.position.y]
            inp = self.line_intersect_circle(ce0,r0, p_1, fp) 
        d1 = [inp[0][0]- local_pose.pose.position.x, inp[0][1] - local_pose.pose.position.y]   # x,y真实位置 X东，Y北  
        d2 = [xt[t+1] - local_pose.pose.position.x, yt[t+1] - local_pose.pose.position.y] 
        vv = [local_vels.twist.linear.x, local_vels.twist.linear.y]        # 真实速度向量   
        if np.dot(d2,d1) < 0:
            d1 = [inp[1][0] - local_pose.pose.position.x, inp[1][1] - local_pose.pose.position.y]
        thta1 = math.atan2(d1[1], d1[0]) 
        thta2 = math.atan2(vv[1], vv[0])
        yta = thta1 - thta2
        self.um = (2 * self.vs**2 / L1) * (math.sin(yta))  # L1制导律公式
        
        v_t = uav_v +0.8* Kv1 * (vt - uav_v)
        v_t = max(min(v_t,v_max),v_min)

                    
        # self.send_cmd2(v = vt, u = self.um, h = self.alt)
        self.v = v_t
        roll_t = math.atan(-self.um/9.80665)
        roll_t = max(min(roll_t,0.8*roll_m),-0.8*roll_m)
        self.roll = 1.0*roll_t
        self.alt = 70    #30  60
        # print(self.alt)
        if self.flag !=10:
            print("--------------------------------------------")
            # print("打击目标的坐标: x = {:7.2f} m, y = {:7.2f} m".format(xt[t],yt[t]))
            print("当前无人机坐标: x = {:7.2f} m, y = {:7.2f} m".format(local_pose.pose.position.x,local_pose.pose.position.y))
            print("无人机速度: {:7.2f} m/s".format(self.v))
            print("无人机滚转角: {:7.2f} deg".format(self.roll*57.3))
            print("无人机高度: {:7.2f} m".format(local_pose.pose.position.z))
            print('目标航点{}: {}'.format(t+1, [xt[t+1],yt[t+1]]))
            print("--------------------------------------------")

            # print(theta_t,local_angles[3],d_theta,w_t)
        '''
        roll_t = math.atan(w_t*uav_v/9.80665)
        roll_t = max(min(roll_t,roll_m),-roll_m)
        '''

        # self.v = v_t
        # self.roll = -1*roll_t
        # self.alt = 40
        seq = local_pose.header.seq*0.02
           
    
    
    def wrap_to_pi(self,theta):
        return (theta + math.pi) % (2 * math.pi) - math.pi

    #防抖
    def sign_replace_function(self,s):
        # eplision = 0.1
        # return s/(abs(s)+eplision)
        eplision = 0.5
        if abs(s/eplision)<=1:
            temp = s/eplision
        else:
            temp = self.sign(s)
        
        return temp

    def sign(self,x):
        if x>0:
            return 1
        elif x==0:
            return 0
        else:
            return -1
    
    # 计算圆 与 线段相交的点
    def line_intersect_circle(self,circle_center,r,point1,point2):
        x0, y0 = circle_center
        x1, y1 = point1[0:2]
        x2, y2 = point2[0:2]
        if x1 == x2:
            inp = []
            if abs(r) >= abs(x1 - x0):
                #下方这个点
                p1 = x1, round(y0 - (r ** 2 - (x1 - x0) ** 2)**(0.5), 5)
                #上方这个点
                p2 = x1, round(y0 +(r ** 2 - (x1 - x0) ** 2)**(0.5), 5)
                inp.append(p2)
                inp.append(p1)

        else:
            #求直线y=kx+b的斜率及b
            k = (y1 - y2) / (x1 - x2)
            b0 = y1 - k * x1
            #直线与圆的方程化简为一元二次方程ax**2+bx+c=0
            a = k ** 2 + 1
            b = 2 * k * (b0 - y0) - 2 * x0
            c = (b0 - y0) ** 2 + x0 ** 2 - r ** 2
            #判别式判断解
            delta = b ** 2 - 4 * a * c
            if delta >= 0:
                p1x = round((-b - delta**(0.5)) / (2 * a), 5)
                p2x = round((-b + delta**(0.5)) / (2 * a), 5)
                p1y = round(k * p1x + b0, 5)
                p2y = round(k * p2x + b0, 5)
                inp = [[p1x, p1y], [p2x, p2y]]
            else:
                inp = []
        return inp       
    
    def Vector3_pack(self, x, y, z):
        # 封装Vector3数据
        data =  Vector3()
        data.x = x
        data.y = y
        data.z = z
        return data
    
    def to_euler_angles(self,w=0, x=0, y=0, z=0):#返回值为弧度制
        angles = {}
        r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
        p = math.asin(2*(w*y-z*x))
        y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))

        angles[1] = r#*180/math.pi
        angles[2] = p#*180/math.pi
        angles[3] = y#*180/math.pi
        #print (r,p,y)
        return angles
    
    def gps2xy(self, lat, lon, ref_lat, ref_lon):
        # input GPS and Reference GPS in degrees
        # output XY in meters (m) X:North Y:East
        CONSTANTS_RADIUS_OF_EARTH = 6371000. # (m)
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        sin_lat = math.sin(lat_rad)
        cos_lat = math.cos(lat_rad)
        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        cos_d_lon = math.cos(lon_rad - ref_lon_rad)

        arg = np.clip(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0)
        c = math.acos(arg)

        k = 1.0
        if abs(c) > 0:
            k = (c / math.sin(c))

        x = float(k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH)
        y = float(k * cos_lat * math.sin(lon_rad - ref_lon_rad) * CONSTANTS_RADIUS_OF_EARTH)

        return y, x
    
    # gps点坐标转换为新gps原点坐标系下的xy坐标，两次gps2xy转换
    def gps_to_xy(self, lat, lon, ref_lat, ref_lon):
        # 原点偏移量
        offset_y, offset_x = self.gps2xy(self.begin_lat, self.begin_lon, self.px4_origin_lat, self.px4_origin_lon)
        # 当前点偏移量
        current_y, current_x = self.gps2xy(lat, lon, ref_lat, rXFD/src/xgc-fixed-wing/src/ef_lon)
        # 转换后的xy坐标
        new_x = current_x + offset_x
        new_y = current_y + offset_y
        return new_y, new_x
        

    # from geographiclib.geocentric import Geocentric
    def ECEF_to_LLA(self, x, y, z):
        geocentric = Geocentric.WGS84()
        lat, lon, alt = geocentric.Reverse(x, y, z)
        return lat, lon, alt
        # self.px4_origin_lat, self.px4_origin_lon, self.px4_origin_alt = self.ECEF_to_LLA(0, 0, 0) 通过gp_origin话题获取ECEF坐标系原点

    def command_callback(self, msg):
        global cnt
        if msg.data == 50:
            self.current_phase = 0
            self.scene = msg.data
            print('Receive command  ', msg.data)
            print('起飞')

        elif msg.data == 51:
            self.current_phase = 2
            self.scene = msg.data
            cnt = 0
            print('Receive command  ', msg.data)
            print("开始执行L1制导律")

        elif msg.data == 52:
            self.current_phase = 11
            self.scene = msg.data
            print('Receive command  ', msg.data)
            print("发现目标，停止执行L1制导律")
        # elif msg.data == 13:
        #     self.state = 10
        #     print('Receive command  ', msg.data)
        #     print("开始模拟目标运动--3对1！！")
        # elif msg.data == 10:
        #     self.state = 11
        #     print('Receive command  ', msg.data)
        #     print("目标被命中！！")
        # elif msg.data == 999:
        #     self.state = 0
        #     print('Receive command  ', msg.data)
        #     print("停止运动！！")

    """
    Main loop.
    """
    def run(self):
        self.wait_for_connection()
        while not rospy.is_shutdown() and self.state.connected:
            self.control()

            # Publish own state for other drones
            # self.state_pub.publish(self.local_position)
            
            rate.sleep()

if __name__ == '__main__':
    try:
        cnt = 0
        pn = 15  # 设置航点数量 与 FP 对应的索引刚好对应上！
        uav_id = int(sys.argv[1]) #这里用的是 1 2 3 4
        rospy.init_node('threeDL1_tracking' + str(uav_id))
        rate = rospy.Rate(50)

        
        xt = [0 for i in range(pn+1)]
        yt = [0 for i in range(pn+1)]
        wb = openpyxl.load_workbook('/home/zrq/Project/XFD/src/search_control/scripts/guidance_tar.xlsx', read_only=True, data_only=True)
        ws = wb['Sheet1']
        for i in range(1,pn+1):
            xt[i] = ws.cell(i,(uav_id-1)*2+1).value
            yt[i] = ws.cell(i,(uav_id-1)*2+2).value
        control_node = L1()
        control_node.run()
    except rospy.ROSInterruptException as e:
        print(e)
    


